//
// Created by Pulsar on 2020/5/16.
//

#include <iostream>
#include <rc_log/slog.hpp>
#include <string>
#include <cstring>
#include <ctime>
#include <vector>
#include <rc_system/data_struct.h>
#include <rc_task/rcTaskVariable.h>
#include <rc_task/rcGyroTask.h>

namespace rccore {
    namespace task {
        void GyroTask::gyro_function(std::vector<char> message) {
            data_struct::CJY901 imu_data;
            char *temp_char = new char[message.size()];
            for (int i = 0; i < (int) message.size() - 1; i++) {
                temp_char[i] = message[i];
            }
            data_struct::STime stcTime{};
            data_struct::SAcc stcAcc{};
            data_struct::SGyro stcGyro{};
            data_struct::SAngle stcAngle{};
            data_struct::SMag stcMag{};
            data_struct::SDStatus stcDStatus{};
            data_struct::SPress stcPress{};
            data_struct::SLonLat stcLonLat{};
            data_struct::SGPSV stcGPSV{};
            std::unique_lock<std::mutex> system_info_lock(task_variable::system_info_mutex);
            switch (message[1]) {
                case 0x50:
                    memcpy(&stcTime, &temp_char[2], 8);
                    imu_data.stcTime = stcTime;
//                    printf("Time:20%d-%d-%d %d:%d:%.3f\r\n", (short) stcTime.ucYear, (short) stcTime.ucMonth,
//                           (short) stcTime.ucDay, (short) stcTime.ucHour, (short) stcTime.ucMinute,
//                           (float) stcTime.ucSecond + (float) stcTime.usMiliSecond / 1000);
                    break;
                case 0x51:
                    memcpy(&stcAcc, &temp_char[2], 8);
                    imu_data.stcAcc = stcAcc;
                    task_variable::systemInfo.IMU_ACC_X = (float) stcAcc.a[0] / 32768 * 16;
                    task_variable::systemInfo.IMU_ACC_Y = (float) stcAcc.a[1] / 32768 * 16;
                    task_variable::systemInfo.IMU_ACC_Z = (float) stcAcc.a[2] / 32768 * 16;
//                    std::cout << "Acc:"
//                              << " " << task_variable::systemInfo.IMU_ACC_X
//                              << " " << task_variable::systemInfo.IMU_ACC_Y
//                              << " " << task_variable::systemInfo.IMU_ACC_Z << "\n";
                    break;
                case 0x52:
                    memcpy(&stcGyro, &temp_char[2], 8);
                    imu_data.stcGyro = stcGyro;
                    task_variable::systemInfo.IMU_GYRO_X = (float) stcGyro.w[0] / 32768 * 2000;
                    task_variable::systemInfo.IMU_GYRO_Y = (float) stcGyro.w[1] / 32768 * 2000;
                    task_variable::systemInfo.IMU_GYRO_Z = (float) stcGyro.w[2] / 32768 * 2000;
//                    std::cout << "Gyro:"
//                              << " " << task_variable::systemInfo.IMU_GYRO_X
//                              << " " << task_variable::systemInfo.IMU_GYRO_Y
//                              << " " << task_variable::systemInfo.IMU_GYRO_Z << "\n";
                    break;
                case 0x53:
                    memcpy(&stcAngle, &temp_char[2], 8);
                    imu_data.stcAngle = stcAngle;
                    task_variable::systemInfo.IMU_ANGLE_X = (float) stcAngle.Angle[0] / 32768 * 180;
                    task_variable::systemInfo.IMU_ANGLE_Y = (float) stcAngle.Angle[1] / 32768 * 180;
                    task_variable::systemInfo.IMU_ANGLE_Z = (float) stcAngle.Angle[2] / 32768 * 180;
//                    std::cout << "Angle:"
//                              << " " << task_variable::systemInfo.IMU_ANGLE_X
//                              << " " << task_variable::systemInfo.IMU_ANGLE_Y
//                              << " " << task_variable::systemInfo.IMU_ANGLE_Z << "\n";
                    break;
                case 0x54:
                    memcpy(&stcMag, &temp_char[2], 8);
                    imu_data.stcMag = stcMag;
                    task_variable::systemInfo.IMU_MAG_X = stcMag.h[0];
                    task_variable::systemInfo.IMU_MAG_Y = stcMag.h[1];
                    task_variable::systemInfo.IMU_MAG_Z = stcMag.h[2];
//                    std::cout << "Mag:"
//                              << " " << stcMag.h[0]
//                              << " " << stcMag.h[1]
//                              << " " << stcMag.h[2] << "\n";
                    break;
                case 0x55:
                    memcpy(&stcDStatus, &temp_char[2], 8);
                    imu_data.stcDStatus = stcDStatus;
//                    std::cout << "DStatus:"
//                              << " " << stcDStatus.sDStatus[0]
//                              << " " << stcDStatus.sDStatus[1]
//                              << " " << stcDStatus.sDStatus[2]
//                              << " " << stcDStatus.sDStatus[3] << "\n";
                    break;
                case 0x56:
                    memcpy(&stcPress, &temp_char[2], 8);
                    imu_data.stcPress = stcPress;
//                    std::cout << "Pressure:"
//                              << " " << stcPress.lPressure
//                              << " " << (float) stcPress.lAltitude / 100 << "\n";
                    break;
                case 0x57:
                    memcpy(&stcLonLat, &temp_char[2], 8);
                    imu_data.stcLonLat = stcLonLat;
                    task_variable::systemInfo.longitude_deg = stcLonLat.lLon / 10000000;
                    task_variable::systemInfo.latitude_deg = stcLonLat.lLat / 10000000;
//                    printf("Longitude:%ldDeg%.5fm Lattitude:%ldDeg%.5fm\r\n",
//                           stcLonLat.lLon / 10000000,
//                           (double) (stcLonLat.lLon % 10000000) / 1e5,
//                           stcLonLat.lLat / 10000000,
//                           (double) (stcLonLat.lLat % 10000000) / 1e5);
                    break;
                case 0x58:
                    memcpy(&stcGPSV, &temp_char[2], 8);
                    imu_data.stcGPSV = stcGPSV;
//                    printf("GPSHeight:%.1fm GPSYaw:%.1fDeg GPSV:%.3fkm/h\r\n\r\n",
//                           (float) stcGPSV.sGPSHeight / 10,
//                           (float) stcGPSV.sGPSYaw / 10, (float) stcGPSV.lGPSVelocity / 1000);
                    break;
            }
            pcontext->pmessage_server->pgyroMessage->push_message(imu_data);
            delete[] temp_char;
            system_info_lock.unlock();
        }

        bool check_function_callback(std::vector<char> package) {
            //数据包大小小于11则校验失败
            if (package.size() < 11)return false;
            return true;
        }

        GyroTask::GyroTask(const std::shared_ptr<common::Context> &pcontext) : BaseTask(pcontext),
                                                                               Serial(pcontext->pconfig->pconfigInfo->GYRO_DEVICE_PATH,
                                                                                      pcontext->pconfig->pconfigInfo->GYRO_DEVICE_FREQ,
                                                                                      pcontext->pconfig->pconfigInfo->GYRO_DEVICE_HEAD) {

        }

        void GyroTask::Run() {
            slog::info << "陀螺仪任务启动中:" << pcontext->pconfig->pconfigInfo->GYRO_DEVICE_PATH << slog::endl;
            if (this->is_open()) {
                tty_num += 1;
                m_is_run = true;
                auto *p_message = new libserv::Message(this->head, 2);
                p_message->BindPackageCheck(check_function_callback);
                messages.push_back(p_message);
                int serial_id = tty_num;
                new std::thread([=]() {
                    fd_set rfds;
                    timeval _timeval{};
                    task::task_variable::TASK_NUM += 1;
                    while (not m_isStop) {
                        std::unique_lock<std::mutex> m_locker(m_mutex);
                        m_cv.wait(m_locker, [this] { return !m_isPause; });
                        FD_ZERO(&rfds);
                        FD_SET(this->fd, &rfds);
                        int retval = select(this->fd + 1, &rfds, NULL, NULL, &_timeval);
                        if (retval == -1)
                            exit(-1);
                        else {
                            if (retval) {
                                std::unique_lock<std::mutex> mu_read_write_port_lock(mu_read_write_port);
                                char *serial_buffer = new char[64];
                                memset(serial_buffer, 0, 64);
                                int nread = read(this->fd, serial_buffer, 1);
                                mu_read_write_port_lock.unlock();
                                if (nread >= 0) {
                                    slog::warn << "(serial_buffer[0]):" << (serial_buffer[0]) << slog::endl;
                                    GyroTask::messages[serial_id]->SerialRecive((serial_buffer[0]));
                                    if (not messages[serial_id]->IsEmpty()) {
                                        std::vector<char> data_package = messages[serial_id]->pop_message();
                                        gyro_function(data_package);
                                    }
                                }
                                delete[] serial_buffer;
                            }
                        }
                        m_locker.unlock();
                        std::this_thread::sleep_for(std::chrono::microseconds(10));
                    }
                    slog::warn << "IMU任务终止" << slog::endl;
                    task::task_variable::TASK_NUM -= 1;
                });
            } else {
                slog::err << "IMU串口打开失败" << slog::endl;
                exit(-1);
            }
        }

        GyroTask::~GyroTask() {
            if (is_run())
                release();
        }
    }
}
